package com.googlecode.gunncs.mechanism.gui;

import java.awt.Color;
import java.awt.Graphics;

import com.googlecode.grtframework.core.GRTLoggedProcess;
import com.googlecode.grtframework.event.EncoderEvent;
import com.googlecode.grtframework.event.EncoderListener;
import com.googlecode.grtframework.sensor.IEncoder;
import com.googlecode.grtframework.vis.Displayable;
import com.googlecode.grtframework.vis.Displayer;
import com.googlecode.grtframework.vis.Mountable;
import com.googlecode.grtframework.vis.MountedPosition;

/**
 * A RobotBaseGUI visualizes robot movement given encoder counts. It can thus
 * "connect" to either a Simulated robot base or a real robot base to visualize
 * movement.
 * 
 * @author ajc
 * 
 */
public class RobotBaseGUI extends GRTLoggedProcess implements Mountable,
		EncoderListener, Displayable {

	// encoders to read drive from
	private final IEncoder lEncoder, rEncoder;
	// wheels to output to
	private DiffWheelGUI lWheel, rWheel;
	private final int wheelDistance;
	// TODO distance per pixel?

	private final Displayer d;

	/**
	 * 
	 * 
	 * @param lEncoder
	 * @param rEncoder
	 * @param start
	 * @param wheelDistance
	 */
	public RobotBaseGUI(Displayer d, IEncoder lEncoder, IEncoder rEncoder,
			MountedPosition start, int wheelDistance, String name) {
		super(name);
		this.d = d;
		this.lEncoder = lEncoder;
		this.rEncoder = rEncoder;
		this.wheelDistance = wheelDistance;

		MountedPosition leftPos = start.clone();
		MountedPosition rWheelPos = start.clone();
		// TODO start at an angle?
		leftPos.setPositionAbsolute(start.getX(), start.getY() - wheelDistance
				/ 2);
		rWheelPos.setPositionAbsolute(start.getX(), start.getY()
				+ wheelDistance / 2);

		lWheel = new DiffWheelGUI(d, leftPos, this);
		rWheel = new DiffWheelGUI(d, rWheelPos, this);

		// debugging
		// MountVisual lM = new MountVisual(d, leftPos, "left");
		// MountVisual rM = new MountVisual(d, rWheelPos, "rWheel");
		// d.addDisplayable(lM);
		// d.addDisplayable(rM);

	}

	@Override
	public double getHeading() {
		// angle between both wheels
		return -(Math.atan2((rWheel.getX() - lWheel.getX()),
				(rWheel.getY() - lWheel.getY())));
	}

	@Override
	public int getX() {
		// midpoint of both wheels
		return (lWheel.getX() + rWheel.getX()) / 2;
	}

	@Override
	public int getY() {
		// midpoint of both wheels
		return (lWheel.getY() + rWheel.getY()) / 2;
	}

	@Override
	public void changedDirection(EncoderEvent e) {
	}

	@Override
	public void countDidChange(EncoderEvent e) {
		if (!suspended) {
			if (e.getSource() == lEncoder) {
				// send left wheel encoder feedback to left wheel
				lWheel.addDistance(e.getDistance());
			} else if (e.getSource() == rEncoder) {
				// send rWheel wheel encoder feedback to rWheel wheel
				rWheel.addDistance(e.getDistance());
			}
		}
	}

	@Override
	public void rotationDidStart(EncoderEvent e) {
	}

	@Override
	public void rotationDidStop(EncoderEvent e) {
	}

	@Override
	public synchronized void initiate() {
		super.initiate();
		lWheel.initiate();
		rWheel.initiate();
		lEncoder.addEncoderListener(this);
		rEncoder.addEncoderListener(this);
		d.addDisplayable(this);

	}

	@Override
	public synchronized void terminate() {
		super.terminate();
		lWheel.terminate();
		rWheel.terminate();
		lEncoder.removeEncoderListener(this);
		rEncoder.removeEncoderListener(this);
		d.removeDisplayable(this);
	}

	@Override
	public void display(Graphics g) {
		g.setColor(Color.GRAY);
		g.fillOval(getX() - wheelDistance / 2, getY() - wheelDistance / 2,
				wheelDistance, wheelDistance);

	}

}
